//
// Created by sundx on 2020/11/9.
//

#include "OffboardManager.h"
#include <boost/bind.hpp>
#include "ros/ros.h"

OffboardManager::OffboardManager(ros::NodeHandle &n) {
    node_name = ros::this_node::getName();
    pkg_path = ros::package::getPath("px4_gcs");
    updateParams(n);
    initState();
    initRosSubPubManager(n);
}

void
OffboardManager::initRosSubPubManager(ros::NodeHandle &n){
    sp_raw_local_pub = n.advertise<mavros_msgs::PositionTarget>(
            mavros_id + "/setpoint_raw/local", 1);
    cmd_request = n.serviceClient<mavros_msgs::SetMode>(mavros_id + "/set_mode");
    arming_client = n.serviceClient<mavros_msgs::CommandBool>(mavros_id + "/cmd/arming");
    local_pose_sub = n.subscribe(
            mavros_id + "/local_position/pose", 1, &OffboardManager::chatterCallback_local_pose, this);
    vision_pose_sub = n.subscribe(
            mavros_id + "/vision_pose/pose", 1, &OffboardManager::chatterCallback_local_pose, this);
    local_vel_sub = n.subscribe(
            mavros_id + "/local_position/velocity_local", 1, &OffboardManager::chatterCallback_local_vel, this);
    mode_sub = n.subscribe(
            mavros_id + "/state", 1, &OffboardManager::chatterCallback_mode, this);
    lan_sub = n.subscribe(
            mavros_id + "/extended_state", 1, &OffboardManager::chatterCallback_extend_state, this);
    ros::spinOnce();
    ROS_INFO("init sub pub finished!!");
}

void
OffboardManager::try_switch_offboard_armed() {
    if (using_gazebo&&init_process_finished) {
        for (int i = 0; i < 3; i++)
        {
            holdZeroPosition();
        }
        setOffboard();
        setArmed();
    }
}

void
OffboardManager::updateParams(ros::NodeHandle &nh) {
    id = RosUtils::readParam<int>(nh,"id");
    rate = RosUtils::readParam<int>(nh, "rate");
    takeoff_height = RosUtils::readParam<float>(nh, "takeoff_height");
    land_speed = RosUtils::readParam<float>(nh, "land_speed");
    mavros_id = RosUtils::readParam<string>(nh, "mavros_id","/mavros");
    using_position_methord = RosUtils::readParam<int>(nh, "using_position_methord");
    using_gazebo = RosUtils::readParam<bool>(nh, "using_gazebo");
    use_takeoff = RosUtils::readParam<bool>(nh, "use_takeoff");
    use_land = RosUtils::readParam<bool>(nh, "use_land");
    use_local_head_target = RosUtils::readParam<bool>(nh, "use_local_head_target");
    ROS_INFO_STREAM("ID: " << id << " mavros_id: " + mavros_id);
}

void
OffboardManager::chatterCallback_local_pose(const geometry_msgs::PoseStamped &msg) {
    cur_pos[0] = msg.pose.position.x;
    cur_pos[1] = msg.pose.position.y;
    cur_pos[2] = msg.pose.position.z;
    tf::quaternionMsgToTF(msg.pose.orientation, cur_att);
    double roll = 0;    double pitch = 0;    double yaw = 0;
    tf::Matrix3x3(cur_att).getRPY(roll, pitch, yaw);
    cur_angle[0] = (float)roll;
    cur_angle[1] = (float)pitch;
    cur_angle[2] = (float)yaw;
//    ROS_INFO_STREAM("pos_x "<<cur_pos[0]);
}

void
OffboardManager::chatterCallback_local_vel(const geometry_msgs::TwistStamped &msg) {
    cur_vel[0] = msg.twist.linear.x;
    cur_vel[1] = msg.twist.linear.y;
    cur_vel[2] = msg.twist.linear.z;
}

void
OffboardManager::chatterCallback_mode(const mavros_msgs::State &msg) {
    if (msg.armed == 1) {
        state.armState = armed;
    } else {
        state.armState = disarmed;
    }
    if (msg.mode == "OFFBOARD") {
        state.flyMode = offboard;
    } else if (msg.mode == "STABILIZED") {
        state.flyMode = stabilized;
    } else if (msg.mode == "ALTCTL") {
        state.flyMode = altctl;
    } else if (msg.mode == "POSCTL") {
        state.flyMode = posctl;
    } else{
        state.flyMode = stabilized;
    }
}


void
OffboardManager::takeoff(float height) {
    if (!take_off_finished) {
        state.airState = takeoffing;
        if (fabs(cur_pos[2] - height) < 0.1) {
            take_off_finished = true;
            return;
        }
        moveto(0,0,height,init_pose[0], init_pose[1], init_pose[2]);
    }
}

void
OffboardManager::holdInitPos(float height) {
    moveto(0,0,height, init_pose[0], init_pose[1], init_pose[2]);
}

void
OffboardManager::land(float target_x, float target_y) {
    if (state.airState == landed) {
        if (state.armState == armed) {
            setStabilized();
            setDisarmed();
        }
    }
    if (!land_finished) {
        ROS_INFO("landing now");
        state.airState = landing;
        //
        if ((cur_pos[2] > -5 || state.airState == landing) && (state.airState != landed)) {
            moveto(target_x, target_y, cur_pos[2] -init_pose[2] - land_speed,
                    init_pose[0],init_pose[1],init_pose[2]);
        } else {
            land_finished = true;
            state.airState = landed;
        }
    }
}

void
OffboardManager::land() {
    if (state.airState == landed) {
        if (state.armState == armed) {
            setDisarmed();
            usleep(100000);//100ms
        }
    }
    if (!land_finished) {
        ROS_INFO("landing now");
        state.airState = landing;
        //
        if ((cur_pos[2] > -5 || state.airState == landing) && (state.airState != landed)) {
            moveToRelativeNow(0,0,-100);
        } else {
            land_finished = true;
            state.airState = landed;
        }
    }else{
        setDisarmed();
        usleep(100000);//100ms
    }
}

void
OffboardManager::holdZeroPosition() {
    moveToRelativeNow(0, 0, 0);
}

void
OffboardManager::moveto(float target_x, float target_y, float target_z){
    moveto(target_x,target_y,target_z,init_pose[0],init_pose[1],init_pose[2]);
}

void
OffboardManager::moveto(float target_x, float target_y, float target_z,
                       float base_x, float base_y, float base_z) {
    if (use_local_head_target) {
        float tx = 0.0;
        float ty = 0.0;
        tx = target_x * cos(init_yaw) - target_y * sin(init_yaw);
        // tx = target_x * cos(init_yaw) + target_y * sin(init_yaw);
        ty = target_x * sin(init_yaw) + target_y * cos(init_yaw);
        // ty = target_x * sin(init_yaw) - target_y * cos(init_yaw);
        target_x = tx;
        target_y = ty;
    }
    sp_pos.header.frame_id = "map";
    // TODO: 测试本处的修改是发送什么坐标系下面的指令
    // sp_pos.coordinate_frame = mavros_msgs::PositionTarget::FRAME_BODY_NED;
    sp_pos.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_NED;
    if (using_control_mode == 0) {
        sp_pos.header.stamp = ros::Time::now();
        sp_pos.position.x = target_x+base_x;
        sp_pos.position.y = target_y+base_y;
        sp_pos.position.z = target_z+base_z;
        sp_pos.type_mask = 0b110111111000;  // 100 111 111 000  xyz + yaw
        sp_raw_local_pub.publish(sp_pos);
        cur_err_dist = getDistanceFrom(target_x+base_x, target_y+base_y, target_z+base_z);
    } else if (using_control_mode == 1) {
        // velocity control no need base
        sp_pos.header.stamp = ros::Time::now();
        sp_pos.velocity.x = target_x;
        sp_pos.velocity.y = target_y;
        sp_pos.velocity.z = target_z;
        sp_pos.type_mask = 0b110111000111;  // 100 111 111 000  xyz + yaw
        sp_raw_local_pub.publish(sp_pos);
    }
}

void
OffboardManager::moveByV(float vx, float vy, float vz){
    sp_pos.header.stamp = ros::Time::now();
    sp_pos.velocity.x = vx;
    sp_pos.velocity.y = vy;
    sp_pos.velocity.z = vz;
    sp_pos.type_mask = 0b110111000111;  // 100 111 111 000  xyz + yaw
    sp_raw_local_pub.publish(sp_pos);
}

void
OffboardManager::moveToRelativeNow(float dx, float dy, float dz) {
    moveto(dx,dy,dz,
           cur_pos[0],
           cur_pos[1],
           cur_pos[2]
    );}


void
OffboardManager::initState() {
    state.emergenceState = normal;
    state.airState = air_state::waitting_takeoff;
    switch (using_position_methord) {
        case 1:
            state.positionState = using_vicon;
            break;
        case 2:
            state.positionState = using_gps;
            break;
        case 3:
            state.positionState = using_att;
            break;
        default:
            break;
    }
}

void
OffboardManager::setOffboard() {
    if(state.flyMode==offboard)return;
    mode_cmd.request.custom_mode = "OFFBOARD";
    cmd_request.call(mode_cmd);
    ROS_INFO("setting OFFBOARD ");
}

void
OffboardManager::setMannual() {
    mode_cmd.request.custom_mode = "MANUAL";
    cmd_request.call(mode_cmd);
    ROS_INFO("setting MANNUAL ");
}

void
OffboardManager::setPosctl() {
    mode_cmd.request.custom_mode = "POSCTL";
    cmd_request.call(mode_cmd);
    ROS_INFO("setting POSCTL ");
}

void
OffboardManager::setAltctl() {
    mode_cmd.request.custom_mode = "ALTCTL";
    cmd_request.call(mode_cmd);
    ROS_INFO("setting ALTCTL ");
}

void
OffboardManager::setStabilized() {
    mode_cmd.request.custom_mode = "STABILIZED";
    cmd_request.call(mode_cmd);
    ROS_INFO("setting STABILIZED ");
}

void
OffboardManager::setArmed() {
    if(state.armState == armed)return;
    arm_cmd.request.value = true;
    arming_client.call(arm_cmd);
    ROS_INFO("setting Armed ");
}

void
OffboardManager::setDisarmed() {
    arm_cmd.request.value = false;
    arming_client.call(arm_cmd);
    ROS_INFO("setting Disarmed ");
}

void
OffboardManager::chatterCallback_extend_state(const mavros_msgs::ExtendedState &msg) {
    if (msg.landed_state == 1) {
        state.airState = landed;
    } else {
        state.airState = in_air;
    }
}

float
OffboardManager::getDistanceFrom(float x, float y, float z) {
    return sqrt((cur_pos[0] - x) * (cur_pos[0] - x) +
                (cur_pos[1] - y) * (cur_pos[1] - y) +
                (cur_pos[2] - z) * (cur_pos[2] - z));
}

vector<float>
OffboardManager::getPositionFromTargetBasedOnHome(vector<float> &target){
    vector<float> a(target.size());
    if(use_local_head_target){
        a[0] = init_pose[0]+target[0] * cos(init_yaw) - target[1] * sin(init_yaw);
        a[1] = init_pose[1]+target[0] * sin(init_yaw) + target[1] * cos(init_yaw);
        a[2] = init_pose[2]+target[2];
    }else{
        a = target;
    }
    return a;
}


void
OffboardManager::init_position(ros::Rate &loop_rate, int average_num) {
    ros::spinOnce(); //获取回调消息
    float _init_pose[3];
    _init_pose[0] = 0.0;
    _init_pose[1] = 0.0;
    _init_pose[2] = 0.0;
    float _init_yaw = 0.0;
    for (int j = 0; j < average_num; ++j) {
        if(fabs(cur_pos[2])<0.000001) {
            ROS_INFO("cur_pos worng!!::%2.4f",cur_pos[2]);
            return;
        }; // remove unstable data;
        holdZeroPosition();
        _init_pose[0] += cur_pos[0];
        _init_pose[1] += cur_pos[1];
        _init_pose[2] += cur_pos[2];
        _init_yaw += cur_angle[2];
        loop_rate.sleep(); //控制帧率
        ros::spinOnce(); //获取回调消息
    }
    init_pose[0] =_init_pose[0]/ (float)average_num;
    init_pose[1] =_init_pose[1]/ (float)average_num;
    init_pose[2] =_init_pose[2]/ (float)average_num;
    init_yaw = _init_yaw/(float)average_num;
    // if(!using_gazebo)init_yaw -= PI/2;
    init_process_finished = true;
    ROS_INFO_STREAM_THROTTLE(1,"ID: " << id << " init position " << init_pose[0] << " " << init_pose[1] << " " << init_pose[2] << " ");
    ROS_INFO_STREAM_THROTTLE(1,"ID: " << id << " init yaw " << init_yaw);
    ROS_INFO_STREAM_THROTTLE(1,"ID: " << id << " init position finished READY TAKEOFF!!!!");
}

std::string
OffboardManager::stateString(run_state _state){
    switch (_state){
        case first_takeoff:
            return "first_takeoff";
        case second_wait_swarm:
            return "second_wait_swarm";
        case third_start_task:
            return "third_start_task";
        case fourth_land:
            return "fourth_land";
    }
}

// 逆时针为正方向 顺时针为反方向
void 
OffboardManager::rotateYawCCW(float target,float base,float x,float y,float z){
    sp_pos.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_NED;
    sp_pos.header.stamp = ros::Time::now();
    sp_pos.yaw = base + target;
    sp_pos.yaw_rate = 0.02;
    sp_pos.position.x = x;
    sp_pos.position.y = y;
    sp_pos.position.z = z;
    sp_pos.type_mask =  mavros_msgs::PositionTarget::IGNORE_AFX | 
                        mavros_msgs::PositionTarget::IGNORE_AFY | 
                        mavros_msgs::PositionTarget::IGNORE_AFZ |
                        mavros_msgs::PositionTarget::IGNORE_VX  | 
                        mavros_msgs::PositionTarget::IGNORE_VY  | 
                        mavros_msgs::PositionTarget::IGNORE_VZ;
    sp_raw_local_pub.publish(sp_pos);
}

void 
OffboardManager::record_pose(){
    last_recorded_pose.x = cur_pos[0];
    last_recorded_pose.y = cur_pos[1];
    last_recorded_pose.z = cur_pos[2];
    last_recorded_pose.yaw = cur_angle[2];
    last_recorded_pose.roll = cur_angle[0];
    last_recorded_pose.pitch  = cur_angle[1];
}

